{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "# SL(4) SLAM with GTSAM\n",
        "\n",
        "This notebook demonstrates a simple 3D SLAM problem using the SL(4) group to represent poses. The scenario is as follows:\n",
        "\n",
        "1. A robot starts at the origin.\n",
        "2. The robot moves in a sequence of 5 poses.\n",
        "3. Odometry measurements are given as `BetweenFactorSL4` factors.\n",
        "4. A loop closure is added between pose 5 and pose 2.\n",
        "\n",
        "We will:\n",
        "- Define noise models.\n",
        "- Create a factor graph representing the problem.\n",
        "- Provide initial estimates for poses.\n",
        "- Optimize the graph using Levenberg-Marquardt to find the most probable configuration."
      ]
    },
    {
      "cell_type": "markdown",
      "id": "colab_button",
      "metadata": {},
      "source": [
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/python/gtsam/examples/SL4SLAMExample.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "license_cell",
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "source": [
        "GTSAM Copyright 2010-2022, Georgia Tech Research Corporation,\nAtlanta, Georgia 30332-0415\nAll Rights Reserved\n\nAuthors: Frank Dellaert, et al. (see THANKS for the full author list)\n\nSee LICENSE for the license information"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 1,
      "metadata": {},
      "outputs": [],
      "source": [
        "try:\n",
        "    import google.colab\n",
        "    %pip install --quiet gtsam-develop\n",
        "except ImportError:\n",
        "    pass"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## The Special Linear Group SL(4)\n",
        "\n",
        "In this example, we use the Special Linear Group SL(4) to represent the robot's pose. SL(4) is the group of 4x4 matrices with determinant 1. In computer vision, 3D homographies are represented by 4x4 matrices, and the group of all invertible 4x4 real matrices is the General Linear Group GL(4). By constraining the determinant to 1, we obtain the SL(4) subgroup. This is analogous to how 2D homographies can be represented by elements of SL(3) to resolve the scale ambiguity."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 2,
      "metadata": {},
      "outputs": [],
      "source": [
        "import numpy as np\n",
        "import gtsam\n",
        "from gtsam import Values, NonlinearFactorGraph, LevenbergMarquardtOptimizer, noiseModel\n",
        "\n",
        "# Import your custom SL4 class and bindings (assumed already wrapped)\n",
        "from gtsam import SL4, PriorFactorSL4, BetweenFactorSL4"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## 1. Setup Factor Graph and Priors"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 3,
      "metadata": {},
      "outputs": [],
      "source": [
        "# Create the factor graph\n",
        "graph = NonlinearFactorGraph()\n",
        "\n",
        "# Prior factor at node 1\n",
        "prior_sigmas = np.full(15, 0.3)\n",
        "prior_noise = noiseModel.Diagonal.Sigmas(prior_sigmas)\n",
        "graph.add(PriorFactorSL4(1, SL4(np.eye(4)), prior_noise))"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## 2. Define the Noise Model and Transformations"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 4,
      "metadata": {},
      "outputs": [],
      "source": [
        "# Between factor noise\n",
        "model_sigmas = np.full(15, 0.2)\n",
        "model = noiseModel.Diagonal.Sigmas(model_sigmas)\n",
        "\n",
        "# Define SL4 transformations (homography matrices)\n",
        "H12 = np.array([\n",
        "    [1.0, 0.1, 0.0, 2.0],\n",
        "    [0.0, 1.0, 0.0, 3.0],\n",
        "    [0.0, 0.0, 1.0, 5.0],\n",
        "    [0.001, 0.002, 0.0, 1.0]\n",
        "])\n",
        "\n",
        "H23 = np.array([\n",
        "    [0.9, 0.2, 0.0, 1.5],\n",
        "    [0.1, 1.1, 0.0, -2.0],\n",
        "    [0.0, 0.0, 0.8, 4.0],\n",
        "    [0.002, 0.003, 0.0005, 1.0]\n",
        "])\n",
        "\n",
        "H34 = np.array([\n",
        "    [1.05, -0.1, 0.0, 3.0],\n",
        "    [0.2, 0.95, 0.0, 1.0],\n",
        "    [0.0, 0.0, 0.9, 2.5],\n",
        "    [0.0015, -0.001, 0.0003, 1.0]\n",
        "])\n",
        "\n",
        "H45 = np.array([\n",
        "    [0.98, 0.05, 0.0, -1.0],\n",
        "    [-0.05, 1.02, 0.0, 2.0],\n",
        "    [0.0, 0.0, 1.1, 0.5],\n",
        "    [0.0008, 0.0015, -0.0002, 1.0]\n",
        "])\n",
        "\n",
        "H52 = np.linalg.inv(H23 @ H34 @ H45)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## 3. Calculate Ground Truth Poses"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 5,
      "metadata": {},
      "outputs": [],
      "source": [
        "# Create SL4 objects\n",
        "H12_SL4 = SL4(H12)\n",
        "H23_SL4 = SL4(H23)\n",
        "H34_SL4 = SL4(H34)\n",
        "H45_SL4 = SL4(H45)\n",
        "H52_SL4 = SL4(H52)\n",
        "H1 = SL4(np.eye(4))\n",
        "\n",
        "# Ground-truth poses\n",
        "H2 = H1.compose(H12_SL4)\n",
        "H3 = H2.compose(H23_SL4)\n",
        "H4 = H3.compose(H34_SL4)\n",
        "H5 = H4.compose(H45_SL4)\n",
        "\n",
        "gt_poses = [H1, H2, H3, H4, H5]"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## 4. Add Odometry Factors"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 6,
      "metadata": {},
      "outputs": [],
      "source": [
        "# Add odometry factors\n",
        "graph.add(BetweenFactorSL4(1, 2, H12_SL4, model))\n",
        "graph.add(BetweenFactorSL4(2, 3, H23_SL4, model))\n",
        "graph.add(BetweenFactorSL4(3, 4, H34_SL4, model))\n",
        "graph.add(BetweenFactorSL4(4, 5, H45_SL4, model))\n",
        "graph.add(BetweenFactorSL4(5, 2, H52_SL4, model))  # loop closure"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## 5. Create Initial Estimates"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 7,
      "metadata": {},
      "outputs": [],
      "source": [
        "# Create initial estimate by perturbing GT\n",
        "initial = Values()\n",
        "rng = np.random.default_rng(seed=42)\n",
        "def random_noise_vector(dim=15):\n",
        "    return rng.uniform(low=-0.1, high=0.1, size=dim)\n",
        "\n",
        "for i, gt_pose in enumerate(gt_poses, 1):\n",
        "    noise = random_noise_vector()\n",
        "    noisy_pose = gt_pose.compose(SL4.Expmap(noise))\n",
        "    initial.insert(i, noisy_pose)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## 6. Optimize"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 8,
      "metadata": {},
      "outputs": [],
      "source": [
        "# Optimize\n",
        "params = gtsam.LevenbergMarquardtParams()\n",
        "params.setRelativeErrorTol(1e-5)\n",
        "params.setMaxIterations(100)\n",
        "optimizer = LevenbergMarquardtOptimizer(graph, initial, params)\n",
        "result = optimizer.optimize()"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## 7. Results"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 9,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Final result:\n",
            "Values with 5 values:\n",
            "Value 1: (gtsam::SL4)\n",
            "           1 -1.49358e-13 -5.67733e-13 -5.17202e-14\n",
            " 3.20484e-15            1 -6.02405e-16 -6.00672e-16\n",
            "-1.08258e-12 -5.88179e-13            1 -1.88337e-13\n",
            " 1.36759e-11  7.42468e-12  2.58617e-11            1\n",
            "\n",
            "Value 2: (gtsam::SL4)\n",
            "     1.00193     0.100193  6.35519e-12      2.00387\n",
            " 6.73137e-12      1.00193  1.05811e-11       3.0058\n",
            " 9.81407e-12 -2.13022e-13      1.00193      5.00967\n",
            "  0.00100193   0.00200387  2.94353e-11      1.00193\n",
            "\n",
            "Value 3: (gtsam::SL4)\n",
            "  0.975878   0.337393  0.0010677    3.52341\n",
            "  0.113176    1.18408 0.00160155     1.0677\n",
            "  0.010677  0.0160155    0.85683     9.6093\n",
            "0.00330987 0.00576558 0.00053385    1.06503\n",
            "\n",
            "Value 4: (gtsam::SL4)\n",
            "    1.12318     0.22456   0.0020653     6.95044\n",
            "   0.365634     1.13859  0.00180304      2.6562\n",
            "  0.0295043  0.00464419     0.79219     12.0763\n",
            "  0.0063721  0.00417704 0.000818743     1.10745\n",
            "\n",
            "Value 5: (gtsam::SL4)\n",
            "    1.06921    0.288659 0.000860931     6.12926\n",
            "   0.296354      1.1557  0.00141783     4.46083\n",
            "  0.0374381   0.0237526    0.848485     12.1583\n",
            " 0.00675841  0.00609308 0.000663101     1.08365\n",
            "\n",
            "\u001b[1;32mSuccessfully optimized!\u001b[0m\n"
          ]
        }
      ],
      "source": [
        "print(\"Final result:\")\n",
        "result.print()\n",
        "\n",
        "# Check accuracy\n",
        "for i, gt_pose in enumerate(gt_poses, 1):\n",
        "    opt_pose = result.atSL4(i)\n",
        "    if not gt_pose.equals(opt_pose, 1e-5):\n",
        "        print(f\"\\033[1;31mPose {i} is outside tolerance!\")\n",
        "print(\"\\033[1;32mSuccessfully optimized!\\033[0m\")"
      ]
    }
  ],
  "metadata": {
    "kernelspec": {
      "display_name": "py312",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.12.6"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 4
}